// #include "mconfig.h"

#include "attitude_estimate.h"
#include "string.h"
#include "math.h"
// #include "serialCan.h"

attitude_typedef attitude = {};


void attitude_value_estimate(float* acc_vehicle, float delta_t)
{
    float acc_world[3] = {};
    float average_motor_speed = 0;
	float r11Andr21[2] = {attitude.DMatrix[0][0],attitude.DMatrix[1][0]} ;
	float zero[2] = {0};
	arm_atan2_f32(- attitude.DMatrix[2][0], arm_euclidean_distance_f32(r11Andr21 ,zero,2),&(attitude.attitude_beta));
	float cbeta = arm_cos_f32(attitude.attitude_beta);
	if (fabs(cbeta)<5){
		attitude.attitude_alpha=0.0f;
		if(attitude.attitude_beta<0){
			arm_atan2_f32(- attitude.DMatrix[0][1],attitude.DMatrix[1][1],&(attitude.attitude_gamma));
		} else arm_atan2_f32(attitude.DMatrix[0][1],attitude.DMatrix[1][1],&(attitude.attitude_gamma));
	}else {
		arm_atan2_f32(attitude.DMatrix[1][0]/cbeta,attitude.DMatrix[0][0]/cbeta,&(attitude.attitude_alpha));
		arm_atan2_f32(- attitude.DMatrix[2][1]/cbeta,attitude.DMatrix[2][2]/cbeta,&(attitude.attitude_gamma)); 
	}
	attitude.attitude_alpha *= 180.0f/PI;
	attitude.attitude_beta *= 180.0f/PI;
	attitude.attitude_gamma *= 180.0f/PI;
	writeAndSendGimbalAngleFD(attitude.attitude_alpha,attitude.attitude_beta,attitude.attitude_gamma);
    arm_mat_vec_mult_f32(&attitude.matrix, acc_vehicle, acc_world);
}


void attitude_init(float* acc, float rate)
{
	attitude.quaternion[0] = 1;
	attitude.matrix.numCols = 3;
	attitude.matrix.numRows = 3;
	attitude.matrix.pData =(float *) attitude.DMatrix;
	attitude.filter_kp = 1;
	attitude.filter_ki = 0.0;
	attitude.attitude_alpha = 0;
	attitude.attitude_beta = 0;
	attitude.attitude_gamma = 0;

	float zero_vector[3] = {};
	float world_z_vector[3] = {0, 0, 1};
	float gravity_vector_body[3] = {acc[0], acc[1], acc[2]};
	float gravity_force = arm_euclidean_distance_f32(gravity_vector_body, zero_vector, 3);
	float body_z_vector[3] = {};
	float cross_product_vector[3] = {};
	float shaft_vector[3] = {};
	arm_scale_f32(gravity_vector_body, - 1 / gravity_force, body_z_vector, 3);
	cross_product_vector[0] = world_z_vector[1] * body_z_vector[2] - world_z_vector[2] * body_z_vector[1];
	cross_product_vector[1] = world_z_vector[2] * body_z_vector[0] - world_z_vector[0] * body_z_vector[2];
	cross_product_vector[2] = world_z_vector[0] * body_z_vector[1] - world_z_vector[1] * body_z_vector[0];
	
	float sin_theta = arm_euclidean_distance_f32(cross_product_vector, zero_vector, 3);
	arm_scale_f32(cross_product_vector, 1 / sin_theta, shaft_vector, 3);
	float theta = asinf(sin_theta);
		
	attitude.quaternion[0] = arm_cos_f32(theta / 2);
	attitude.quaternion[1] = - arm_sin_f32(theta / 2) * shaft_vector[0];
	attitude.quaternion[2] = - arm_sin_f32(theta / 2) * shaft_vector[1];
	attitude.quaternion[3] = - arm_sin_f32(theta / 2) * shaft_vector[2];
	arm_quaternion2rotation_f32(attitude.quaternion, attitude.matrix.pData, 1);
	attitude.rate = rate;
	attitude.initOK  = 1;
}

void attitude_execute(float* gyro_rate, float* acc)
{	
	if(attitude.rate == 0 || attitude.initOK !=1)return;
	float omega_vector[3] = {gyro_rate[0], gyro_rate[1], gyro_rate[2]};
	float omega_quaternion[4] = {}; 
	float zero_vector[3] = {};
	float gravity_vector_body[3] = {acc[0], acc[1], acc[2]};
	float gravity_force = arm_euclidean_distance_f32(gravity_vector_body, zero_vector, 3);
	float gravity_vector_world[3] = {0, 0, -1};
	float gravity_vector_gyro[3] = {};
	float gravity_cross_product[3] = {};
	
	float error_p_vector[3] = {};
	float error_i_vector[3] = {};
	float error_vector[3] = {};
	
	if(gravity_force >= 9 && gravity_force <= 10.4){

		float matrix_g_w_data[9] = {};
		arm_matrix_instance_f32 matrix_g_w = {.numCols = 3, .numRows = 3, .pData = matrix_g_w_data};
		arm_mat_trans_f32(&attitude.matrix, &matrix_g_w);
		arm_mat_vec_mult_f32(&matrix_g_w, gravity_vector_world, gravity_vector_gyro);
		
		arm_scale_f32(gravity_vector_body, 1 / gravity_force, gravity_vector_body, 3);
		gravity_cross_product[0] = gravity_vector_gyro[1] * gravity_vector_body[2] - gravity_vector_gyro[2] * gravity_vector_body[1];
		gravity_cross_product[1] = gravity_vector_gyro[2] * gravity_vector_body[0] - gravity_vector_gyro[0] * gravity_vector_body[2];
		gravity_cross_product[2] = gravity_vector_gyro[0] * gravity_vector_body[1] - gravity_vector_gyro[1] * gravity_vector_body[0];
		
		float sin_theta = arm_euclidean_distance_f32(gravity_cross_product, zero_vector, 3);
		float theta = asinf(sin_theta);
		if(sin_theta != 0)
		{
			arm_scale_f32(gravity_cross_product, 1 / attitude.rate, error_i_vector, 3);
			arm_add_f32(attitude.filter_error_integral, error_i_vector, attitude.filter_error_integral, 3);
			arm_scale_f32(gravity_cross_product, attitude.filter_kp, error_p_vector, 3);
			arm_scale_f32(attitude.filter_error_integral, attitude.filter_ki, error_i_vector, 3);
			arm_add_f32(error_p_vector, error_i_vector, error_vector, 3);
			arm_scale_f32(error_vector, -1, error_vector, 3);
			arm_add_f32(omega_vector, error_vector, omega_vector, 3);
		}
	}
	memcpy(omega_quaternion + 1, omega_vector, sizeof(float) * 3);
	
	float quaternion_derivative[4] = {};
	
	attitude_typedef attitude_gyro = {};
	attitude_gyro.quaternion[0] = 1;
	attitude_gyro.matrix.numCols = 3;
	attitude_gyro.matrix.numRows = 3;
	attitude_gyro.matrix.pData = attitude_gyro.matrix_data;
	memcpy(attitude_gyro.matrix_data, attitude.matrix_data, sizeof(float) * 9);
	arm_quaternion_product_single_f32(attitude.quaternion, omega_quaternion, quaternion_derivative);
	arm_scale_f32(quaternion_derivative, 0.5 / attitude.rate, quaternion_derivative, 4);
	arm_add_f32(attitude.quaternion, quaternion_derivative, attitude_gyro.quaternion, 4);
	arm_quaternion_normalize_f32(attitude_gyro.quaternion, attitude.quaternion, 1);
	arm_quaternion2rotation_f32(attitude_gyro.quaternion, attitude.matrix.pData, 1);


	attitude_value_estimate(acc,attitude.rate);
}

